/*! @file camera_gimbal_sample.cpp
 *  @version 3.3
 *  @date Jun 05 2017
 *
 *  @brief
 *  Camera and Gimbal Control API usage in a Linux environment.
 *  Shows example usage of camera commands and gimbal position/speed control
 *  APIs
 *
 *  @Copyright (c) 2017 DJI
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "camera_gimbal_sample.hpp"

using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;

extern CamPos DJI_Z3_Pos;
extern RotationAngle initialAngle;
Vehicle*   vehicle;
int G_mission_statues = 0;

#define SEARCH_MODE 0
#define TRACT_MODE  1

extern int fd;


static int right_extend = 0;
static int left_extend = 0;
static RotationAngle currentAngle;
static RotationAngle lastAngle;
static float currentStepAngleTotal = 0;


static TeleMag currentMag;
static TeleQuat currentQuat;

//Normalize angle to be within the interval [-180,180].
inline int standardRad(int t)
{
    if (t >= 0)
    {
        t = fmod(t+180, 360) - 180;
    }
    else
    {
        t = fmod(t-180, -360) + 180;
    }
    return t;
}


void * gimbalCameraControl(void *)
{
    //sim
    //DJI_Z3_Pos.gimbal_mode = SEARCH_MODE;

    if(!vehicle->gimbal)
    {
        DERROR("Gimbal object does not exist.\n");
        //return false;
        pthread_exit((void *)"Gimbal thread error");
    }
    DJI::OSDK::Gimbal::SpeedData gimbalSpeed;
    DJI::OSDK::Gimbal::AngleData gimbalAngle;

    lastAngle.roll = currentAngle.roll;
    lastAngle.pitch = currentAngle.pitch;
    lastAngle.yaw = currentAngle.yaw;
    currentAngle.roll  = vehicle->broadcast->getGimbal().roll;
    currentAngle.pitch = vehicle->broadcast->getGimbal().pitch;
    currentAngle.yaw   = vehicle->broadcast->getGimbal().yaw;

    std::cout<<"currentAngle.yaw:"<<currentAngle.yaw<<std::endl;
    std::cout<<"initialAngle.yaw:"<<initialAngle.yaw<<std::endl;

    while(1)
    {
        if(!DJI_Z3_Pos.gimbal_mode) //search_mode
        {
            //reset to initial angle
            gimbalAngle.roll=0;
            gimbalAngle.pitch=-890;
            gimbalAngle.yaw=0;
            gimbalAngle.mode = 1;
            gimbalAngle.duration = 10;
            vehicle->gimbal->setAngle(&gimbalAngle);
            sleep(2);

            int break_flag2 = 0;
            int break_flag1 = 0;
            for(int i=-4; i<=0 ;i++) //i=-4
            {
                for(int j=-9; j<9 ; j++)
                {
                    gimbalAngle.roll=0;
                    int yaw_dir = (i%2)*2+1;
                    //gimbalAngle.pitch=i*180;
                    //gimbalAngle.yaw=j*195*yaw_dir;  //j*10 * (i == )
                    gimbalAngle.yaw=-90;
                    gimbalAngle.pitch=0;
                    gimbalAngle.mode = 1;
                    gimbalAngle.duration = 10;   //units 0.1s
                    vehicle->gimbal->setAngle(&gimbalAngle);
                    //usleep(100000);
                    for(int k=0; k<12; k++)
                    {
                        usleep(100000);
                        //usleep(1000);
                        if(DJI_Z3_Pos.tar_detec_flag == 1)
                        {
                            DJI_Z3_Pos.gimbal_mode = TRACT_MODE;
                            i = 0;
                            j = 0;
                            break_flag2 = 1;

                            //std::cout<<"k:"<<k<<endl;
                            //0502:ensure that take 2second part into 20*100ms is useful for immediately break.
                            break;
                        }
                        if(G_mission_statues)
                        {
                            DJI_Z3_Pos.gimbal_mode = TRACT_MODE;
                            i = 0;
                            j = 0;
                            break_flag2 = 1;
                            break;
                        }
                    }
                    if(break_flag2 == 1)
                    {
                        break_flag1 == 1;
                        break;
                    }

//                    currentAngle.roll  = vehicle->broadcast->getGimbal().roll;
//                    currentAngle.pitch = vehicle->broadcast->getGimbal().pitch;
//                    currentAngle.yaw   = vehicle->broadcast->getGimbal().yaw;

//                    std::cout<<"currentAngle.yaw:"<<currentAngle.yaw<<std::endl;
//                    std::cout<<"currentAngle.pitch:"<<currentAngle.pitch<<std::endl;
//                    std::cout<<"currentAngle.roll:"<<currentAngle.roll<<std::endl;

//                    currentMag.x   = vehicle->broadcast->getMag().x;
//                    currentMag.y   = vehicle->broadcast->getMag().y;
//                    currentMag.z   = vehicle->broadcast->getMag().z;
//                    std::cout<<"currentMag.x:"<<currentMag.x<<std::endl;
//                    std::cout<<"currentMag.y:"<<currentMag.y<<std::endl;
//                    std::cout<<"currentMag.z:"<<currentMag.z<<std::endl;

//                    currentQuat.q0 = vehicle->broadcast->getQuaternion().q0;
//                    currentQuat.q1 = vehicle->broadcast->getQuaternion().q1;
//                    currentQuat.q2 = vehicle->broadcast->getQuaternion().q2;
//                    currentQuat.q3 = vehicle->broadcast->getQuaternion().q3;

//                    std::cout<<"currentQuat.q0:"<<currentQuat.q0<<std::endl;
//                    std::cout<<"currentQuat.q1:"<<currentQuat.q1<<std::endl;
//                    std::cout<<"currentQuat.q2:"<<currentQuat.q2<<std::endl;
//                    std::cout<<"currentQuat.q3:"<<currentQuat.q3<<std::endl;

                    //std::cout<<"gimbalAngle.yaw:"<<gimbalAngle.yaw<<std::endl;
                    //std::cout<<"gimbalAngle.pitch:"<<gimbalAngle.pitch<<std::endl;
                }
                if(break_flag1 == 1)
                {
                    break;
                }
            }
        }
        else //tract_mode
        {
            //std::cout<<"yawLimit"<<int(vehicle->broadcast->getGimbal().yawLimit)<<std::endl;

            double yaw_direction=0;
            double pitch_direction=0; //(DJI_Z3_Pos.PosMcircle.y-360)>0;
            if((640-DJI_Z3_Pos.PosMcircle.x)>20)
            {
                yaw_direction = -(640.0-DJI_Z3_Pos.PosMcircle.x)/640.0; //-1-0
            }
            else if((640-DJI_Z3_Pos.PosMcircle.x)<-20)
            {
                yaw_direction = (DJI_Z3_Pos.PosMcircle.x-640.0)/640.0; //0-1
            }
            else
            {
                yaw_direction = 0;
            }

            if((360-DJI_Z3_Pos.PosMcircle.y)>20)
            {
                pitch_direction = (360.0-DJI_Z3_Pos.PosMcircle.y)/360.0; //0-1
            }
            else if((360-DJI_Z3_Pos.PosMcircle.y)<-20)
            {
                pitch_direction = -(DJI_Z3_Pos.PosMcircle.y-360.0)/360.0; //-1-0
            }
            else
            {
                pitch_direction = 0;
            }

            // Speed control
            //  std::cout << "Gimbal Speed Description: \n\n"
            //            << "Roll - unit 0.1 degrees/second input rate [-1800, 1800]\n"
            //            << "Pitch - unit 0.1 degrees/second input rate [-1800, 1800]\n"
            //            << "Yaw - unit 0.1 degrees/second input rate [-1800, 1800]\n\n";
            gimbalSpeed.roll  = 0;
            gimbalSpeed.pitch = 300*pitch_direction;
            gimbalSpeed.yaw   = 300*yaw_direction;
            gimbalSpeed.gimbal_control_authority = 1;
            gimbalSpeed.disable_fov_zoom = 0;
            gimbalSpeed.ignore_user_stick = 0;
            gimbalSpeed.extend_control_range = 0;
            gimbalSpeed.ignore_aircraft_motion = 0;
            gimbalSpeed.yaw_return_neutral = 0;
            gimbalSpeed.reserved0 = 0;
            gimbalSpeed.reserved1 = 0;
            vehicle->gimbal->setSpeed(&gimbalSpeed);
            usleep(1000);   //  wait for vehicle recive the message

            //0503 solve limit
            static int limit_flag_last;
            static int limit_flag_current;
            static int limit_flag_times;
            static int limit_flag;
            limit_flag_last = limit_flag_current;
            if(int(vehicle->broadcast->getGimbal().yawLimit) == 1)
            {
                limit_flag_current = 1;
            }
            else
            {
                limit_flag_current = 0;
            }

            if(limit_flag_current == 1 && limit_flag_last ==1)
            {
                limit_flag_times ++;
            }
            else
            {
                limit_flag_times = 0;
            }

            if(limit_flag_times > 4)
            {
                limit_flag = 1;
            }
            else
            {
                limit_flag = 0;
            }

            if(limit_flag)
            //if(1)
            {
                limit_flag_current = 0;
                float x = DJI_Z3_Pos.PosMcircle.x;
                float y = DJI_Z3_Pos.PosMcircle.y;

                currentAngle.roll  = vehicle->broadcast->getGimbal().roll;
                currentAngle.pitch = vehicle->broadcast->getGimbal().pitch;
                currentAngle.yaw   = vehicle->broadcast->getGimbal().yaw;

                //speed mode
//                gimbalSpeed.roll  = 0;
//                gimbalSpeed.pitch = 0;
//                gimbalSpeed.yaw   = -900;
//                vehicle->gimbal->setSpeed(&gimbalSpeed);
//                usleep(1000);
//                sleep(4);

//                if(int(vehicle->broadcast->getGimbal().yawLimit))
//                {
//                    gimbalSpeed.roll  = 0;
//                    gimbalSpeed.pitch = 0;
//                    gimbalSpeed.yaw   = 900;
//                    vehicle->gimbal->setSpeed(&gimbalSpeed);
//                    usleep(1000);
//                    sleep(4);
//                }


                //increasement mode
                gimbalAngle.roll=0;
                gimbalAngle.pitch=0;
                gimbalAngle.yaw=3600;
                gimbalAngle.mode = 0;
                gimbalAngle.duration = 10;
                vehicle->gimbal->setAngle(&gimbalAngle);
                sleep(1);
//                gimbalAngle.roll=0;
//                gimbalAngle.pitch=0;
//                gimbalAngle.yaw=1800;
//                gimbalAngle.mode = 0;
//                gimbalAngle.duration = 10;
//                vehicle->gimbal->setAngle(&gimbalAngle);
//                sleep(1);


                if(int(vehicle->broadcast->getGimbal().yawLimit))
                {
                    gimbalAngle.roll=0;
                    gimbalAngle.pitch=0;
                    gimbalAngle.yaw=-3600;
                    gimbalAngle.mode = 0;
                    gimbalAngle.duration = 10;
                    vehicle->gimbal->setAngle(&gimbalAngle);
                    sleep(1);

//                    gimbalAngle.roll=0;
//                    gimbalAngle.pitch=0;
//                    gimbalAngle.yaw=-1800;
//                    gimbalAngle.mode = 0;
//                    gimbalAngle.duration = 10;
//                    vehicle->gimbal->setAngle(&gimbalAngle);
//                    sleep(1);

                }

//                DJI_Z3_Pos.PosMcircle.x = x;
//                DJI_Z3_Pos.PosMcircle.y = y;
                DJI_Z3_Pos.gimbal_mode = TRACT_MODE;
            }

//            currentAngle.roll  = vehicle->broadcast->getGimbal().roll;
//            currentAngle.pitch = vehicle->broadcast->getGimbal().pitch;
//            currentAngle.yaw   = vehicle->broadcast->getGimbal().yaw;
//            std::cout<<"currentAngle.yaw:"<<currentAngle.yaw<<std::endl;
//            std::cout<<"currentAngle.pitch:"<<currentAngle.pitch<<std::endl;
        }

        if(G_mission_statues)
        {
            pthread_exit((void *)"Gimbal thread done");
        }
    }
//    displayResult(&currentAngle);
    //std::cout<<"DJI_Z3_Pos.gimbal_mode"<<DJI_Z3_Pos.gimbal_mode<<std::endl;
//    std::cout<<"vehicle->broadcast->getGimbal().yawLinit"<<int(vehicle->broadcast->getGimbal().yawLimit)<<std::endl;
}


void displayResult(RotationAngle* currentAngle)
{
  std::cout << "New Gimbal rotation angle is [";
  std::cout << currentAngle->roll << " ";
  std::cout << currentAngle->pitch << " ";
  std::cout << currentAngle->yaw;
  std::cout << "]\n\n";
}
